#include <eeprom.h>
#include <ecrt/ioctl.h>
#include <ecrt/master.h>
#include <cstring>
#include <sys/ioctl.h>
EEprom::EEprom()
{
}

EEprom::~EEprom()
{
}

/**
 * @Fn:
 *      eeprom_locate_start_of_conf
 * @Berif:
 *      Return the start address of a EtherCAT config area in eeprom. 
 * */
uint8_t* EEprom::eeprom_locate_start_of_conf(uint8_t *data, uint16_t cfg)
{
    eeprom_conf_header_t *hdr;
    uint8_t *cur;

    if (data == NULL)
        return NULL;

    cur = data + 0x80;
    while (1) {
        hdr = (eeprom_conf_header_t *)cur;
        if (hdr->cfg == cfg) {
            return cur;
        } else if (hdr->cfg == 0xffff) {
            break;
        }
        cur += sizeof(eeprom_conf_header_t);
        cur += hdr->len * sizeof(uint16_t);
    }

    fprintf(stderr, "Failed to locate the start address of cfg 0x04%x.\n", cfg);
    return NULL;
}


/* Get the number of syc manager of a slave */
uint16_t EEprom::eeprom_abtain_sync_count(uint8_t *data)
{
    eeprom_conf_header_t *hdr;
    uint8_t *cur;
    uint16_t sm_cnt;

    cur = eeprom_locate_start_of_conf(data, EEPROM_SYNC_MNGR);
    if (cur == NULL) {
        return 0;
    }

    hdr = (eeprom_conf_header_t *)cur;
    sm_cnt =  hdr->len * sizeof(uint16_t) / sizeof(eeprom_sync_t);

    return sm_cnt;
}

/* Get the number of pdo of a sync manager */
uint16_t EEprom::eeprom_abtain_pdo_count_of_sync(uint8_t *data, uint16_t sm_idx)
{
    uint8_t *end, *cur;
    eeprom_conf_header_t *hdr;
    eeprom_pdo_t *pdo;
    uint16_t pdo_count;
    uint16_t sm_idx_hist;
    uint8_t idx;


    for (idx = 0, pdo_count = 0; idx < 2; idx++) {
        if (idx == 0) {
            cur = eeprom_locate_start_of_conf(data, EEPROM_PDO_TX);
            sm_idx_hist = TX_SYNC_INDEX;
        } else {
            cur = eeprom_locate_start_of_conf(data, EEPROM_PDO_RX);
            sm_idx_hist = RX_SYNC_INDEX;
        }
        if (cur == NULL) {
            return 0;
        }

        hdr = (eeprom_conf_header_t *)cur;
        cur += sizeof(eeprom_conf_header_t);
        end = cur + hdr->len * sizeof(uint16_t);

        while (cur < end) {
            pdo = (eeprom_pdo_t *)cur;
#if 0
            /* If the sm_index of a pdo is not set, 0xff by default,
             * set it to the previose sync manager index. */
            if (pdo->sm_index != 0xff)
                sm_idx_hist = pdo->sm_index;
            else
#endif
            pdo->sm_index = sm_idx_hist;

            if (pdo->sm_index == sm_idx)
                pdo_count++;
                
            cur += sizeof(eeprom_pdo_t);
            cur += pdo->n_entries * sizeof(eeprom_pdo_entry_t);
        }
    }

    return pdo_count;
}

int EEprom::eeprom_aggr_sync_init(uint8_t *data, eeprom_aggr_sync_t *aggr_sync, uint16_t sync_count)
{
    eeprom_sync_t *eeprom_sync;
    uint8_t *cur;
    uint16_t sm_idx;


    cur = eeprom_locate_start_of_conf(data, EEPROM_SYNC_MNGR);
    if (cur == NULL) {
        return -1;
    }

    cur += sizeof(eeprom_conf_header_t);
    eeprom_sync = (eeprom_sync_t *)cur;

    for (sm_idx = 0; sm_idx < sync_count; sm_idx++) {
        aggr_sync[sm_idx].eeprom_sync = eeprom_sync + sm_idx;
    }


    return 0;
}

int EEprom::eeprom_aggr_pdo_init(uint8_t *data, eeprom_aggr_sync_t *aggr_sync, uint16_t sync_count)
{
    eeprom_conf_header_t *hdr;
    eeprom_pdo_t *eeprom_pdo;
    eeprom_aggr_pdo_t *aggr_pdo;
    uint8_t *end, *cur;
    uint16_t idx, sm_idx, *idx_p, *sync_pdo_idx;

    sync_pdo_idx = (uint16_t*)malloc(sizeof(uint16_t) * sync_count);
    if (sync_pdo_idx == NULL) {
        fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
        return -1;
    }
    memset(sync_pdo_idx, 0, sizeof(uint16_t) * sync_count);

    for (idx = 0; idx < 2; idx++) {
        if (idx == 0) {
            cur = eeprom_locate_start_of_conf(data, EEPROM_PDO_TX);
        } else {
            cur = eeprom_locate_start_of_conf(data, EEPROM_PDO_RX);
        }
        if (cur == NULL) {
            free(sync_pdo_idx);
            return -2;
        }

        hdr = (eeprom_conf_header_t *)cur;
        cur += sizeof(eeprom_conf_header_t);
        end = cur + hdr->len * sizeof(uint16_t);

        while (cur < end) {
            eeprom_pdo = (eeprom_pdo_t *)cur;
            sm_idx = eeprom_pdo->sm_index;
            idx_p = sync_pdo_idx + sm_idx;
            aggr_pdo = aggr_sync[sm_idx].aggr_pdo + *idx_p;
            aggr_pdo->eeprom_pdo = eeprom_pdo;
            aggr_pdo->entry_count = eeprom_pdo->n_entries;
            cur += sizeof(eeprom_pdo_t);
            aggr_pdo->eeprom_entries = (eeprom_pdo_entry_t *)cur;
            cur += eeprom_pdo->n_entries * sizeof(eeprom_pdo_entry_t);
            (*idx_p) += 1;
        }
    }

    free(sync_pdo_idx);

    return 0;
}


int EEprom::eeprom_aggr_init(uint8_t *data, eeprom_aggr_sync_t *aggr_sync, uint16_t sync_count)
{
    if (eeprom_aggr_sync_init(data, aggr_sync, sync_count) < 0)
        return -1;
    if (eeprom_aggr_pdo_init(data, aggr_sync, sync_count) < 0)
        return -2;

    return 0;
}

/* Caculate the number of all of PDOs' entries in SII, without repeatability. */
int EEprom::eeprom_dsync_get_pdos_entry_count(eeprom_aggr_sync_t *aggr_sync)
{
	eeprom_aggr_sync_t *ea_sync;
	eeprom_aggr_pdo_t *ea_pdo;
	eeprom_pdo_entry_t *entries;
	uint16_t *nmem;
	uint32_t len;
	uint16_t entry_cnt;
	uint16_t sm_idx, pdo_idx, entry_idx, idx = 0;
	int32_t count;

	/* Caculate numbers of pdo_entries. */
	entry_cnt = 0;
	for (sm_idx = 0; ; sm_idx++) {
		ea_sync = aggr_sync + sm_idx;
		if (ea_sync->eeprom_sync == NULL)
			break;
		if (ea_sync->pdo_count == 0)
			continue;
		for (pdo_idx = 0; pdo_idx < ea_sync->pdo_count; pdo_idx++) {
			ea_pdo = &ea_sync->aggr_pdo[pdo_idx];
			entry_cnt += ea_pdo->entry_count;
		}
	}

	len = sizeof(uint16_t) * entry_cnt;
	nmem = (uint16_t *)malloc(len);
	if (nmem == NULL) {
		fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
		return -1;
	}
	memset((uint8_t*)nmem, 0, len);

	for (sm_idx = 0, count = 0; ; sm_idx++) {
		ea_sync = aggr_sync + sm_idx;
		if (ea_sync->eeprom_sync == NULL)
			break;
		if (ea_sync->pdo_count == 0)
			continue;
		for (pdo_idx = 0; pdo_idx < ea_sync->pdo_count; pdo_idx++) {
			ea_pdo = ea_sync->aggr_pdo + pdo_idx;
			entries = ea_pdo->eeprom_entries;
			entry_cnt = ea_pdo->entry_count;
			for (entry_idx = 0; entry_idx < entry_cnt; entry_idx++) {
				/* By default, the PDO entry of max torque is ignored. */
				if (entries[entry_idx].index == EC_PDO_MAX_TORQUE_INDEX) {
					continue;
				}

				/* FIXME : The PDO entry of 0x603f is deleted, evenify the
				 * number of all bit_lengh of all PDO entries.
				 *
				 * This should be fix later. */
				if (entries[entry_idx].index == 0x603f) {
					continue;
				}

				for(idx = 0; idx < count; idx++) {
					if (nmem[idx] == entries[entry_idx].index)
						break;
				}
				if (nmem[idx])
					continue;

				nmem[idx] = entries[entry_idx].index;
				count += 1;
			}
		}
	}

	free(nmem);

	return count;
}


int EEprom::eeprom_dsync_sync_allocate(eeprom_aggr_sync_t *aggr_sync, ec_sync_info_t **sync)
{
    uint8_t *nmem;
    uint32_t len;
    uint16_t entry_cnt;

    entry_cnt = eeprom_dsync_get_pdos_entry_count(aggr_sync);
    if (entry_cnt < 0) {
        fprintf(stderr, \
            "%s(): Failed to allocate the number of PDO entries.\n",
            __func__);
        return -1;
    }

    /* allocate memory for Sync Managers and PDOs */

    /* a redundant instance at the end, and it will be initiated as 0xff. */
    len = sizeof(ec_sync_info_t) * (DEFAULT_SYNC_COUNT + 1);
    len += sizeof(ec_pdo_info_t) * DEFAULT_PDO_COUNT;
    nmem = (uint8_t *)malloc(len);
    if (nmem == NULL) {
        fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
        return -2;
    }

    memset(nmem, 0, len);
    *sync = (ec_sync_info_t *)nmem;
    nmem += sizeof(ec_sync_info_t) * (DEFAULT_SYNC_COUNT + 1);
    (*sync)[RX_SYNC_INDEX].pdos = (ec_pdo_info_t *)nmem;
    nmem += sizeof(ec_pdo_info_t);
    (*sync)[TX_SYNC_INDEX].pdos = (ec_pdo_info_t *)nmem;

    /* allocate memory for PDO entries */
    len = sizeof(ec_pdo_entry_info_t) * entry_cnt;
    nmem = (uint8_t *)malloc(len);
    if (nmem == NULL) {
        fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
        return -3;
    }
    memset(nmem, 0, len);
    (*sync)[RX_SYNC_INDEX].pdos[0].entries = (ec_pdo_entry_info_t *)nmem;
    return 0;
}

int EEprom::eeprom_dsync_sync_init(eeprom_aggr_sync_t *aggr_sync, ec_sync_info_t *sync)
{
    uint16_t sm_idx;

#if 0
    nmem = (uint8_t *)sync;
    nmem += sizeof(ec_sync_info_t) * (DEFAULT_SYNC_COUNT + 1);
    ec_pdo = (ec_pdo_info_t*)nmem;
    nmem += sizeof(ec_pdo_info_t) * DEFAULT_PDO_COUNT;
#endif

    /* initiate sync managers */
    for (sm_idx = 0; sm_idx < DEFAULT_SYNC_COUNT; sm_idx++) {
        sync[sm_idx].index = sm_idx;
        if (sm_idx < 2)
            continue;
        sync[sm_idx].n_pdos = 1;
        //sync[sm_idx].pdos = ec_pdo;
        //ec_pdo += 1;
    }
    sync[sm_idx].index = 0xff;
    //sync[2].pdos[0].entries = (ec_pdo_entry_info_t*)nmem;

    return 0;
}

/* Caculate the number of all of PDOs' entries belonging to a sync manager in SII,
 * without repeatability. */
int EEprom::eeprom_dsync_get_sync_pdos_entry_count(eeprom_aggr_sync_t *aggr_sync, uint16_t sm_idx)
{
	eeprom_aggr_sync_t *ea_sync;
	eeprom_aggr_pdo_t *ea_pdo;
	eeprom_pdo_entry_t *entries;
	uint16_t *nmem;
	uint32_t len;
	uint16_t entry_cnt;
	uint16_t pdo_idx, entry_idx, idx = 0;
	int32_t count;

	/* Caculate numbers of pdo_entries. */
	ea_sync = aggr_sync + sm_idx;
	if (ea_sync->eeprom_sync == NULL)
		return 0;
	if (ea_sync->pdo_count == 0)
		return 0;

	for (pdo_idx = 0, entry_cnt = 0; pdo_idx < ea_sync->pdo_count; pdo_idx++) {
		ea_pdo = &ea_sync->aggr_pdo[pdo_idx];
		entry_cnt += ea_pdo->entry_count;
	}

	len = sizeof(uint16_t) * entry_cnt;
	nmem = (uint16_t *)malloc(len);
	if (nmem == NULL) {
		fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
		return -1;
	}
	memset((uint8_t*)nmem, 0, len);

	ea_sync = aggr_sync + sm_idx;
	for (pdo_idx = 0, count = 0; pdo_idx < ea_sync->pdo_count; pdo_idx++) {
		ea_pdo = ea_sync->aggr_pdo + pdo_idx;
		entries = ea_pdo->eeprom_entries;
		entry_cnt = ea_pdo->entry_count;
		for (entry_idx = 0; entry_idx < entry_cnt; entry_idx++) {
			/* By default, the PDO entry of max torque is ignored. */
			if (entries[entry_idx].index == EC_PDO_MAX_TORQUE_INDEX) {
				continue;
			}
			/* FIXME : The PDO entry of 0x603f is deleted */
			if (entries[entry_idx].index == 0x603f) {
				continue;
			}
			for(idx = 0; idx < count; idx++) {
				if (nmem[idx] == entries[entry_idx].index)
					break;
			}
			if (nmem[idx])
				continue;
			nmem[idx] = entries[entry_idx].index;
			count += 1;
		}
	}

	free(nmem);

	return count;
}

/* To initiate the PDOs and their entries. */
int EEprom::eeprom_dsync_pdo_and_entries_init(eeprom_aggr_sync_t *aggr_sync, ec_sync_info_t *sync)
{
	ec_pdo_info_t *ec_pdo;
	ec_pdo_entry_info_t *ec_entries;
	eeprom_aggr_sync_t *ea_sync;
	eeprom_aggr_pdo_t *ea_pdo;
	eeprom_pdo_entry_t *ea_entries;
	uint16_t pdo_cnt, entry_cnt, count;
	uint16_t pdo_idx, entry_idx, idx, i;

	count = eeprom_dsync_get_pdos_entry_count(aggr_sync);
	ec_entries = sync[2].pdos[0].entries;
	/* initiate PDOs */
	for (idx = 0, i = 0; idx < DEFAULT_PDO_COUNT; idx++) {
		ec_pdo = sync[2].pdos + idx;
		ec_pdo->entries = ec_entries + idx;
		if (idx == 0) {
			ec_pdo->n_entries = eeprom_dsync_get_sync_pdos_entry_count(aggr_sync, 2);
			ea_sync = aggr_sync + 2;
		} else {
			ea_sync = aggr_sync + 3;
			sync[3].pdos[0].entries = ec_entries + sync[2].pdos[0].n_entries;
			ec_pdo->n_entries = eeprom_dsync_get_sync_pdos_entry_count(aggr_sync, 3);
		}

		pdo_cnt = ea_sync->pdo_count;
		for (pdo_idx = 0; pdo_idx < pdo_cnt; pdo_idx++) {
			ea_pdo = ea_sync->aggr_pdo + pdo_idx;
			entry_cnt = ea_pdo->entry_count;
			if (ec_pdo->index == 0) {
				/* IF content of this PDO is not fixed,
				 * use it as a PDO in sync manager. */
				if ((ea_pdo->eeprom_pdo->flags & 0x0010) == 0) {
					ec_pdo->index = ea_pdo->eeprom_pdo->index;
				}
			}

			for (entry_idx = 0; entry_idx < entry_cnt; entry_idx++) {
				ea_entries = ea_pdo->eeprom_entries + entry_idx;
				for (i = 0; i < count; i++) {
					/* By default, the PDO entry of max torque is ignored. */
					if (ea_entries->index == EC_PDO_MAX_TORQUE_INDEX) {
						break;
					}
					/* FIXME : The PDO entry of 0x603f is deleted */
					if (ea_entries->index == 0x603f) {
						break;
					}
					if (ec_entries[i].index == ea_entries->index) {
						break;
					}
					if (ec_entries[i].index == 0) {
						ec_entries[i].index = \
							ea_entries->index;
						ec_entries[i].subindex = \
							ea_entries->subindex;
						ec_entries[i].bit_length = \
							ea_entries->bit_length;
						break;
					}
				}
			}
		}
	}

	return 0;
}

int EEprom::eeprom_realloc_rx_pdos_entries(uint8_t *nmem, ec_pdo_entry_info_t *tmp_entries, uint8_t *odd, uint8_t *even, int odd_cnt, int even_cnt, int spec_idx)
{
    int arr_idx, i, len;
    ec_pdo_entry_info_t *ec_entries, *new_entries;

    /*if 8bits pdo_entries's index is odd, and have 0x6060,so copy 0x6060*/
    if ((odd_cnt % 2) && spec_idx){
            for (i = 0; i < odd_cnt; i++){
                arr_idx = odd[i];
                ec_entries = tmp_entries + arr_idx;
                if (ec_entries->index == 0x6060){
                    new_entries = ec_entries;
                    memcpy(nmem, (uint8_t *)new_entries, sizeof(ec_pdo_entry_info_t));
                    nmem += sizeof(ec_pdo_entry_info_t);
                    continue;
                }
                memcpy(nmem, (uint8_t *)ec_entries, sizeof(ec_pdo_entry_info_t));
                nmem += sizeof(ec_pdo_entry_info_t);
        }
    }

    /*if 8 bits pdo_entries's index is odd, and no 0x6060*/
    if ((odd_cnt % 2) && (spec_idx == 0)){
        for (i = 0; i < odd_cnt; i++){
            arr_idx = odd[i];
            ec_entries = tmp_entries + arr_idx;
            memcpy(nmem, (uint8_t *)ec_entries, sizeof(ec_pdo_entry_info_t));
            nmem += sizeof(ec_pdo_entry_info_t);
        }
        len = sizeof(ec_pdo_entry_info_t);
        new_entries = (ec_pdo_entry_info_t *)malloc(len);
        if (new_entries == NULL){
            fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
            return -1;
        }
        memset(new_entries, 0, len);

        new_entries->index = 0x6060;
        new_entries->subindex = 0x0;
        new_entries->bit_length = 0x8;
        memcpy(nmem, (uint8_t *)new_entries, sizeof(ec_pdo_entry_info_t));
        nmem += sizeof(ec_pdo_entry_info_t);

        free(new_entries);
    }

    /*if 8 bits pdo_entries's index is even, and have 0x6060*/
    if ((odd_cnt % 2 == 0) && spec_idx){
        return 0;
    }

    /*if 8 bits pdo_entries's index is even, and no 0x6060*/
    if ((odd_cnt % 2 == 0) && (spec_idx == 0)){
        len = sizeof(ec_pdo_entry_info_t);
        new_entries = (ec_pdo_entry_info_t *)malloc(len);
        if (new_entries == NULL){
            fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
            return -1;
        }
        memset(new_entries, 0, len);

        new_entries->index = 0x6060;
        new_entries->subindex = 0x0;
        new_entries->bit_length = 0x8;
        memcpy(nmem, (uint8_t *)new_entries, sizeof(ec_pdo_entry_info_t));
        nmem += sizeof(ec_pdo_entry_info_t);
        memcpy(nmem, (uint8_t *)new_entries, sizeof(ec_pdo_entry_info_t));
        nmem += sizeof(ec_pdo_entry_info_t);

        free(new_entries);
    }
    return 0;
}

int EEprom::eeprom_realloc_tx_pdos_entries(uint8_t *nmem, ec_pdo_entry_info_t *tmp_entries, uint8_t *odd,uint8_t *even, int odd_cnt, int even_cnt)
{
    int i, arr_idx;

    ec_pdo_entry_info_t *ec_entries;
    /*if 8 bits pdo_entries's index is odd */
     if (odd_cnt % 2 ){
         for (i = 0; i < odd_cnt; i++){
             arr_idx = odd[i];
             ec_entries = tmp_entries + arr_idx;
             memcpy(nmem, (ec_pdo_entry_info_t *)ec_entries, sizeof(ec_pdo_entry_info_t));
             nmem += sizeof(ec_pdo_entry_info_t);
         }
     }
     return 0;
}

int EEprom::eeprom_dsync_pdo_alloc_entries(ec_sync_info_t *sync)
{
    int i, entry_idx, entry_cnt, arr_idx, len;
    int spec_idx = 0;
    ec_pdo_entry_info_t *ec_entries, *tmp_entries, *old_entries;
    uint8_t *nmem, idx, tmp_cnt;
    uint8_t even[16] = {0};
    uint8_t odd[16] = {0};

    // 应该是将所有的entries信息都放到了这个里面了
    old_entries = sync[RX_SYNC_INDEX].pdos[0].entries;
    for (idx = RX_SYNC_INDEX; idx < DEFAULT_SYNC_COUNT; idx++) {
//        uint8_t even_cnt = 0;
//        uint8_t odd_cnt = 0;
        tmp_entries = ec_entries = sync[idx].pdos[0].entries;
        tmp_cnt = entry_cnt = sync[idx].pdos[0].n_entries;

//        for (entry_idx = 0; entry_idx < entry_cnt; entry_idx++){
//            if (ec_entries[entry_idx].bit_length == 0x08){
//                if (ec_entries[entry_idx].index == 0x6060)
//                    spec_idx = 1;
//                odd[odd_cnt] = entry_idx;
//                odd_cnt++;
//            } else {
//                even[even_cnt] = entry_idx;
//                even_cnt ++;
//            }
//        }

//        if (odd_cnt % 2) {
//            tmp_cnt++;
//            std::cout << "odd_cnt % 2 " << tmp_cnt << std::endl;
//            sync[idx].pdos[0].n_entries = tmp_cnt;
//        } else if ((odd_cnt == 0) && (spec_idx == 0)) {
//            tmp_cnt = tmp_cnt + 2;
//            std::cout << "odd_cnt == 0 " << tmp_cnt << std::endl;
//            sync[idx].pdos[0].n_entries = tmp_cnt;
//        }

        len = sizeof(ec_pdo_entry_info_t) * tmp_cnt;
        nmem = (uint8_t *)malloc(len);
        if (nmem == NULL){
            fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
            return -1;
        }
        memset(nmem, 0, len);
        sync[idx].pdos[0].entries = (ec_pdo_entry_info_t *)nmem;

//        for (i = 0; i < even_cnt; i++){
//            arr_idx = even[i];
//            ec_entries = tmp_entries + arr_idx;
//            memcpy(nmem, (uint8_t *)ec_entries, sizeof(ec_pdo_entry_info_t));
//            nmem += sizeof(ec_pdo_entry_info_t);
//        }
//
//        for (i = 0; i < odd_cnt; i++){
//            arr_idx = odd[i];
//            ec_entries = tmp_entries + arr_idx;
//            memcpy(nmem, (uint8_t *)ec_entries, sizeof(ec_pdo_entry_info_t));
//            nmem += sizeof(ec_pdo_entry_info_t);
//        }
          memcpy(nmem, tmp_entries, tmp_cnt * sizeof(ec_pdo_entry_info_t));

//        if (idx == 2) {
//            eeprom_realloc_rx_pdos_entries(nmem, tmp_entries, odd, even, odd_cnt, even_cnt, spec_idx);
//        } else if(idx == 3){
//            eeprom_realloc_tx_pdos_entries(nmem, tmp_entries, odd, even, odd_cnt, even_cnt);
//        }
    }
    free(old_entries);
    return 0;
}

/**
 * @Fn:
 *      eeprom_aggr_create_and_init
 * @Berif:
 *      To parse the slave info read from a slave's eeprom, and to
 *      allocate and initiate the corresponding eeprom_agg_sync_t
 *      instances and eeprom_agg_pdo_t instances.
 *
 * @Input: 
 *      eeprom_data: the data of eeprom
 * @Output: 
 *      aggr_sync： The data parsed from eeprom is placed inside
 *      success, return 0; or, return -1
 */
int32_t EEprom::eeprom_aggr_create_and_init(uint8_t *eeprom_data, eeprom_aggr_sync_t **aggr_sync)
{
	eeprom_aggr_sync_t *sm = NULL;
	uint8_t *nmem, len;
	uint32_t ret;
	uint16_t sm_idx, sm_cnt = 0;
	

	sm_cnt = eeprom_abtain_sync_count(eeprom_data);
	if (sm_cnt == 0)
		return -1;

	/* Add a redundant instance at the end, to sign the end*/
	len = sizeof(eeprom_aggr_sync_t) * (sm_cnt + 1);
	nmem = (uint8_t *)malloc(len);
	if (nmem == NULL) {
		fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
		return -2;
	}
	memset(nmem, 0, len);
	sm = (eeprom_aggr_sync_t*)nmem;

	/* caculate the number of pdos belong to a sync manager */
	for (sm_idx = 0; sm_idx < sm_cnt; sm_idx++) {
		sm[sm_idx].pdo_count = \
			eeprom_abtain_pdo_count_of_sync(eeprom_data, sm_idx);
	}

	/* allocate eeprom_aggr_pdo_t instances, which are used to referring the
	 * corresponding pdo instances and their PDO entries. */
	for (sm_idx = 0; sm_idx < sm_cnt; sm_idx++) {
		if (sm[sm_idx].pdo_count == 0)
			continue;
		len = sizeof(eeprom_aggr_pdo_t) * sm[sm_idx].pdo_count;
		nmem = (uint8_t *)malloc(len);
		if (nmem == NULL) {
			fprintf(stderr, "%s(): Failed to allocate new memory.\n", __func__);
			ret = -3;
			goto Failed_clear_garbage;
		}
		memset(nmem, 0, len);
		sm[sm_idx].aggr_pdo = (eeprom_aggr_pdo_t*)nmem;
		nmem = NULL;
	}


	if (eeprom_aggr_init(eeprom_data, sm, sm_cnt) < 0 ) {
		fprintf(stderr, "%s(): Failed to aggr instances.\n", __func__);
		ret = -4;
		goto Failed_clear_garbage;
	}

	*aggr_sync = sm;

	return 0;

Failed_clear_garbage:
	if (sm) {
		free(sm);
	}
	for (sm_idx = 0; sm_idx < sm_cnt; sm_idx++) {
		if (sm[sm_idx].aggr_pdo) {
			free(sm[sm_idx].aggr_pdo);
		}
	}

	return ret;
}

/**
 * @Fn: 
 *      eeprom_get_slave_sii_words
 * @Berif:
 *      Read the information of the corresponding slave from the eeprom according to the index number of the slave
 * @Input:
 *      master_fd: the fd of master
 *      slave_index: the index of current slave
 * @Output:
 *      success, return 0; or, return -1
 */
uint8_t* EEprom::eeprom_get_slave_sii_words(int master_fd, uint32_t slave_index)
{
    ec_ioctl_slave_t io_slave;
    ec_ioctl_slave_sii_t io_sii;
    
    memset((char*)(&io_slave), 0, sizeof(ec_ioctl_slave_t));
    io_slave.position = slave_index;
    if (ioctl(master_fd, EC_IOCTL_SLAVE, &io_slave)) {
        fprintf(stderr, "%s: failed to get IOCTL info of slave-%d", \
            __func__, slave_index);
        return NULL;
    }
    
    memset((char*)(&io_sii), 0, sizeof(ec_ioctl_slave_sii_t));
    io_sii.words = (uint16_t*)malloc(io_slave.sii_nwords * 2);
    if (io_sii.words == NULL) {
        fprintf(stderr, "%s: failed to allocate new memory/\n", __func__);
        return NULL;
    }
    memset((char *)io_sii.words, 0, io_slave.sii_nwords * 2);

    io_sii.slave_position = slave_index;
    io_sii.offset = 0;
    io_sii.nwords = io_slave.sii_nwords;
    if (ioctl(master_fd, EC_IOCTL_SLAVE_SII_READ, &io_sii)) {
        fprintf(stderr, "%s: failed to get sii info of slave-%d", \
            __func__, slave_index);
        free(io_sii.words);
        return NULL;
    }
    
    return (uint8_t *)(io_sii.words);
}

/**
 * @Fn:
 *      eeprom_dsync_create_and_init
 * @Berif:
 *      According to the contents of eeprom, memory allocation and initialization for sync
 * @Input:
 *      aggr_sync: Data obtained from eeprom parsing
 * @Output:
 *      sync: the sync address of current slave
 *      success, return 0; or, return -1
 */
int EEprom::eeprom_dsync_create_and_init(eeprom_aggr_sync_t *aggr_sync, ec_sync_info_t **sync)
{
    if (eeprom_dsync_sync_allocate(aggr_sync, sync) < 0)
        return -1;
  
    eeprom_dsync_sync_init(aggr_sync, *sync);
    eeprom_dsync_pdo_and_entries_init(aggr_sync, *sync);
    eeprom_dsync_pdo_alloc_entries(*sync);
    return 0;
}

/**
 * @Fn:
 *      eeprom_adjust_sync
 * @Berif:
 *      Complement sync synced from eeprom
 * @Input:
 *      slave_index: the index of current slave
 *      slave_sync : the sync point of current slave
 * @Output:
 *      success, return 0; or, return -1;
 */
int EEprom::eeprom_adjust_sync(ec_master_t* master, uint32_t slave_index, ec_sync_info_t* slave_sync)
{
    uint32_t sync_idx, sync_count;
    ec_slave_info_t slave_info;
    ec_sync_info_t  sync;
    if(ecrt_master_get_slave(master, slave_index, &slave_info)) {
            fprintf(stderr, "Failed to abtain slave-%d information\n", (slave_index + 1));
            return -1;
    }

    sync_count = slave_info.sync_count;
    for (sync_idx = 0; sync_idx < sync_count; ++ sync_idx) {
        if (ecrt_master_get_sync_manager(master, slave_index, sync_idx, &sync)) {
            fprintf(stderr, "Failed to abtain sync_info%04d of slave%04d information\n", sync_idx, slave_index);
            return -2;
        }

        slave_sync[sync_idx].dir = sync.dir;
        slave_sync[sync_idx].watchdog_mode = sync.watchdog_mode;
    }

    return 0;
}

/**
 * @Fn:
 *      eeprom_get_slave_sync
 * @Berif:
 *      Read the eeprom content and parse out the sync of the corresponding slave according to the content.
 * @Input:
 *      slave_index: the index of current slave
 *      slave_sync : the sync address of slave
 * @Output:
 *      success, return 0; or, return -1
 */
bool EEprom::GetSyncInfo(ec_sync_info_t** slave_sync, uint32_t slave_index, ec_master_t* master)
{
    uint8_t *eeprom_data = NULL;
    eeprom_aggr_sync_t *aggr_syncs = NULL;
    uint32_t master_fd = master->fd;

    eeprom_data = eeprom_get_slave_sii_words(master_fd, slave_index);
    if(!eeprom_data){
        std::cerr << "eeprom_get_slave_sii_words failed!!!" << std::endl;
        return false;
    }

    if(eeprom_aggr_create_and_init(eeprom_data, &aggr_syncs) < 0){
        std::cerr << "eeprom_aggr_create_and_init failed!!!" << std::endl;
        return false;
    }

    if(eeprom_dsync_create_and_init(aggr_syncs, slave_sync) < 0){
        std::cerr << "eeprom_dsync_create_and_init failed!!!" << std::endl;
        return false;
    }

    if(eeprom_adjust_sync(master, slave_index, *slave_sync)){
        std::cerr << "eeprom adjust sync failed!!!" << std::endl;
        return false;
    }
    return true;
}

